// Copyright (c) Acconeer AB, 2022-2024
// All rights reserved
// This file is subject to the terms and conditions defined in the file
// 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
// of this source code package.

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include "acc_definitions_a121.h"
#include "acc_detector_distance.h"
#include "acc_hal_definitions_a121.h"
#include "acc_hal_integration_a121.h"
#include "acc_integration.h"
#include "acc_integration_log.h"
#include "acc_rss_a121.h"
#include "acc_sensor.h"
#include "acc_version.h"

#include "user_app.h"

#include <string.h>
#include "crc.h"
#include "usart.h"

extern UART_HandleTypeDef huart1;

extern A121_distance_config_t  A121_distance_config;


//uint16_t


/** \example example_detector_distance.c
 * @brief This is an example on how the Detector Distance API can be used
 * @n
 * This example executes as follows:
 *   - Retrieve HAL integration
 *   - Initialize distance detector resources:
 *     + Create distance detector configuration
 *     + Update configuration settings
 *     + Create Distance detector handle
 *     + Create buffer for detector calibration data
 *     + Create buffer for sensor data
 *   - Create and calibrate the sensor
 *   - Calibrate the detector
 *   - Measure distances with the detector (loop):
 *     + Prepare sensor with the detector
 *     + Measure and wait until a read can be done
 *     + Process measurement and print the result
 *     + Handle "calibration_needed" indication
 *   - acc_cleanup:
 *     + Destroy detector configuration
 *     + Destroy detector handle
 *     + Destroy sensor data buffer
 *     + Destroy detector calibration data buffer
 */





typedef struct
{
	acc_sensor_t                      *sensor;
	acc_detector_distance_config_t    *config;
	acc_detector_distance_handle_t    *handle;
	void                              *buffer;
	uint32_t                          buffer_size;
	uint8_t                           *detector_cal_result_static;
	uint32_t                          detector_cal_result_static_size;
	acc_detector_cal_result_dynamic_t detector_cal_result_dynamic;
} acc_distance_detector_resources_t;

acc_cal_result_save_t  acc_cal_result_save={0};


static void acc_cleanup(acc_distance_detector_resources_t *resources);


static bool acc_initialize_detector_resources(acc_distance_detector_resources_t *resources);


static bool acc_do_sensor_calibration(acc_sensor_t     *sensor,
                                  acc_cal_result_t *sensor_cal_result,
                                  void             *buffer,
                                  uint32_t         buffer_size);


static bool acc_do_full_detector_calibration(acc_distance_detector_resources_t *resources,
                                         const acc_cal_result_t        *sensor_cal_result);


static bool acc_do_detector_calibration_update(acc_distance_detector_resources_t *resources,
                                           const acc_cal_result_t        *sensor_cal_result);


static bool acc_do_detector_get_next(acc_distance_detector_resources_t  *resources,
                                 const acc_cal_result_t         *sensor_cal_result,
                                 acc_detector_distance_result_t *result);


static void acc_print_distance_result(const acc_detector_distance_result_t *result);

static void acc_print_iq_data(const acc_config_t        *sensor_config,
                          acc_processing_result_t   *processing_result,
                          acc_processing_metadata_t *processing_metadata);

int acc_distance(int argc, char *argv[]);



void reset_distance_setting(void)
{
	A121_distance_config.start=0.25f;
	A121_distance_config.end=3.0f;
	A121_distance_config.step_length=0U;
	A121_distance_config.profile=ACC_CONFIG_PROFILE_5;
	A121_distance_config.reflector_shape=ACC_DETECTOR_DISTANCE_REFLECTOR_SHAPE_GENERIC;
	A121_distance_config.peak_sorting=ACC_DETECTOR_DISTANCE_PEAK_SORTING_STRONGEST;
	A121_distance_config.threshold_method=ACC_DETECTOR_DISTANCE_THRESHOLD_METHOD_CFAR;
	A121_distance_config.num_frames_recorded=100U;
	A121_distance_config.threshold_sensitivity=0.5f;
	A121_distance_config.signal_quality=15.0f;
	A121_distance_config.close_cancellation=false;
	A121_distance_config.scan_nub=1;
	A121_distance_config.average=0;
	A121_distance_config.data_output_time=100;

	acc_cal_result_save.use_cal_caching_flag=0;//默认不启用
	acc_cal_result_save.cal_flag=0;
	acc_cal_result_save.acc_recal=0;

}

void printf_distance_config(void)
{
	printf("Distance configuration:\n");
	printf("     detection range :%.2f~%.2f m\n",A121_distance_config.start,A121_distance_config.end);
	printf("          step length=%2d,                profile=%d\n",A121_distance_config.step_length,A121_distance_config.profile);
	printf("     reflector_shape =%1d,                average=%d\n",A121_distance_config.reflector_shape,A121_distance_config.average);
//	printf("     threshold_method=%d,    num_frames_recorded=%d\n",A121_distance_config.threshold_method,A121_distance_config.num_frames_recorded);
	printf("threshold_sensitivity=%.2f,      signal_quality=%.2f\n",A121_distance_config.threshold_sensitivity,A121_distance_config.signal_quality);
	printf("   close_cancellation=%d,       data_output_time=%dms\n",A121_distance_config.close_cancellation,A121_distance_config.data_output_time);
	printf("             scan_nub=%d,           peak_sorting=%d\n",A121_distance_config.scan_nub,A121_distance_config.peak_sorting);
	printf("  num_frames_recorded=%3d,   calibration_caching=%d\n",A121_distance_config.num_frames_recorded,acc_cal_result_save.use_cal_caching_flag);
}

static void acc_set_config(acc_detector_distance_config_t *detector_config)
{
	// Add configuration of the detector here

	acc_detector_distance_config_start_set(detector_config, A121_distance_config.start);//0.25f);
	acc_detector_distance_config_end_set(detector_config, A121_distance_config.end);//3.0f);
	acc_detector_distance_config_max_step_length_set(detector_config, 0);//A121_distance_config.step_length);//0U);//使用Cfar需设置为0  （自动分配步长）
	if(A121_distance_config.profile==0)
	{
		acc_detector_distance_config_max_profile_set(detector_config, ACC_CONFIG_PROFILE_5);
	}
	else
	{
		acc_detector_distance_config_max_profile_set(detector_config, A121_distance_config.profile);
	}
	acc_detector_distance_config_reflector_shape_set(detector_config, A121_distance_config.reflector_shape);//ACC_DETECTOR_DISTANCE_REFLECTOR_SHAPE_GENERIC);
	acc_detector_distance_config_peak_sorting_set(detector_config, A121_distance_config.peak_sorting);//ACC_DETECTOR_DISTANCE_PEAK_SORTING_STRONGEST);
	acc_detector_distance_config_threshold_method_set(detector_config, A121_distance_config.threshold_method);//ACC_DETECTOR_DISTANCE_THRESHOLD_METHOD_CFAR);
	acc_detector_distance_config_num_frames_recorded_threshold_set(detector_config, A121_distance_config.num_frames_recorded);//100U);
	acc_detector_distance_config_threshold_sensitivity_set(detector_config, A121_distance_config.threshold_sensitivity);//0.5f);
	acc_detector_distance_config_signal_quality_set(detector_config, A121_distance_config.signal_quality);//15.0f);
	acc_detector_distance_config_close_range_leakage_cancellation_set(detector_config, A121_distance_config.close_cancellation);//false);


}


// Calibration and caching functions
//static bool acc_sensor_and_detector_calibration(distance_detector_resources_t *resources, uint16_t cache_index)
//{
//	bool status = sensor_calibration(resources, cache_index);
//
//	if (status)
//	{
//		status = acc_do_full_detector_calibration(resources, cache_index);
//
//	}
//
//	if (status)
//	{
//		acc_cal_info_t cal_info;
//		acc_sensor_get_cal_info(&sensor_cal_results[cache_index], &cal_info);
//		add_cache(cache_index, cal_info.temperature);
//		cache_index_in_use = cache_index;
//	}
//
//	return status;
//}


uint16_t  save_buf_size=0;

uint8_t updata_flag=0;

volatile acc_cal_result_t sensor_cal_result1;
volatile acc_distance_detector_resources_t resources1 = { 0 };

uint32_t TT=0;
uint8_t   st_buf[2048]={0};

int acc_distance(int argc, char *argv[])
{
	(void)argc;
	(void)argv;
	uint8_t update_flag=0;
	acc_cal_result_t sensor_cal_result;
	acc_distance_detector_resources_t resources = { 0 };

	if(A121_distance_config.run_status==0)
	{
		distance_save_data();
		return 0;
	}
//	printf("Acconeer software version %s\n", acc_version_get());

	const acc_hal_a121_t *hal = acc_hal_rss_integration_get_implementation();

	if (!acc_rss_hal_register(hal))
	{
		return EXIT_FAILURE;
	}

	resources.config = acc_detector_distance_config_create();
	if (resources.config == NULL)
	{
		printf("acc_detector_distance_config_create() failed\n");
		acc_cleanup(&resources);
		return EXIT_FAILURE;
	}

	acc_set_config(resources.config);

	if (!acc_initialize_detector_resources(&resources))
	{
		printf("Initializing detector resources failed\n");
		acc_cleanup(&resources);
		return EXIT_FAILURE;
	}

	// Print the configuration
//	acc_detector_distance_config_log(resources.handle, resources.config);

	/* Turn the sensor on */
	acc_hal_integration_sensor_supply_on(1);
	acc_hal_integration_sensor_enable(1);

	resources.sensor = acc_sensor_create(1);
	if (resources.sensor == NULL)
	{
		printf("acc_sensor_create() failed\n");
		acc_cleanup(&resources);
		return EXIT_FAILURE;
	}

//	acc_cal_result_t sensor_cal_result;
	uint32_t T1;// = HAL_GetTick();
	uint32_t T2;// = HAL_GetTick();
	uint16_t i;
	T1 = HAL_GetTick();


	//跳过校准
		if((acc_cal_result_save.use_cal_caching_flag==1)&&(acc_cal_result_save.cal_flag==1))
		{
//			sensor_cal_result=sensor_cal_result1;

			for( i=0;i<acc_cal_result_save.close_buf_size;i++)
			{
				resources.detector_cal_result_static[i]=acc_cal_result_save.close_buf[i];
			}
			for( i=0;i<acc_cal_result_save.temp_buf_size;i++)
			{
				sensor_cal_result.data[i]=acc_cal_result_save.temp_buf[i];
			}
			if (acc_do_detector_calibration_update(&resources, &sensor_cal_result))
			{
				update_flag=1;
			}

			T2 = HAL_GetTick();
			TT=T2-T1;
			if(update_flag==1)
			{
				printf("skip calibration use time=%dms\r\n",TT);
			}
		}
		if(update_flag!=1)
		{

			if (!acc_do_sensor_calibration(resources.sensor, &sensor_cal_result, resources.buffer, resources.buffer_size))
			{
				printf("Sensor calibration failed\n");
				acc_cleanup(&resources);
				return EXIT_FAILURE;
			}
			if (!acc_do_full_detector_calibration(&resources, &sensor_cal_result))
			{
				printf("Detector calibration failed\n");
				acc_cleanup(&resources);
				return EXIT_FAILURE;
			}

			if(acc_cal_result_save.use_cal_caching_flag==1)
			{
				//近距离校准数据
				for( i=0;i<resources.detector_cal_result_static_size;i++)
				{
					acc_cal_result_save.close_buf[i]=resources.detector_cal_result_static[i];
				}
				for(i=0;i<(ACC_CAL_RESULT_DATA_SIZE/4);i++)
				{
					acc_cal_result_save.temp_buf[i]=sensor_cal_result.data[i];
				}
				acc_cal_result_save.close_buf_size=resources.detector_cal_result_static_size;
				acc_cal_result_save.temp_buf_size=ACC_CAL_RESULT_DATA_SIZE/4;

				acc_cal_result_save.cal_flag=1;
				A121_distance_config.save_flag=1;
				distance_save_data();
			}

			T2 = HAL_GetTick();
			TT=T2-T1;
			printf("full_detector_calibration use time=%dms\r\n",TT);
		}

	uint32_t tickstart1;// = HAL_GetTick();
	uint32_t tickstart2;// = HAL_GetTick();
	uint32_t Ttemp=0;
	uint16_t delayT=10;
	acc_detector_distance_result_t result = { 0 };

	while (A121_distance_config.run_status)
//	if(updata_flag==0){delayT=20;updata_flag=1;}else{delayT=2000;}
//
//	for(uint16_t t=0;t<delayT;t++)
	{

		tickstart1 = HAL_GetTick();
		if (!acc_do_detector_get_next(&resources, &sensor_cal_result, &result))
		{
			printf("Could not get next result\n");
			acc_cleanup(&resources);
//			return EXIT_FAILURE;
		}


		/* If "calibration needed" is indicated, the sensor needs to be recalibrated and the detector calibration updated */
		if (result.calibration_needed)
		{
			printf("Sensor recalibration and detector calibration update needed ... \n");

			if (!acc_do_sensor_calibration(resources.sensor, &sensor_cal_result, resources.buffer, resources.buffer_size))
			{
				printf("Sensor calibration failed\n");
				acc_cleanup(&resources);
				return EXIT_FAILURE;
			}

			/* Once the sensor is recalibrated, the detector calibration should be updated and measuring can continue. */
			if (!acc_do_detector_calibration_update(&resources, &sensor_cal_result))
			{
				printf("Detector calibration update failed\n");
				acc_cleanup(&resources);
				return EXIT_FAILURE;
			}

			printf("Sensor recalibration and detector calibration update done!\n");
		}
		else
		{
			acc_print_distance_result(&result);
		}

		LED2_tog

		Free_IWDG();
		distance_save_data();
		tickstart2 = HAL_GetTick();
		Ttemp=tickstart2-tickstart1;
		if(A121_distance_config.data_output_time>Ttemp)
		{
			HAL_Delay(A121_distance_config.data_output_time-Ttemp);
		}

		//重新校准
		if(acc_cal_result_save.acc_recal==1)
		{
			acc_cal_result_save.acc_recal=0;
			acc_cleanup(&resources);
			return EXIT_SUCCESS;
		}
	}


	acc_cleanup(&resources);
	printf("Stop!\n");
	return EXIT_SUCCESS;


}


static void acc_cleanup(acc_distance_detector_resources_t *resources)
{
	acc_hal_integration_sensor_disable(1);
	acc_hal_integration_sensor_supply_off(1);

	acc_detector_distance_config_destroy(resources->config);
	acc_detector_distance_destroy(resources->handle);

	acc_integration_mem_free(resources->buffer);
	acc_integration_mem_free(resources->detector_cal_result_static);

	if (resources->sensor != NULL)
	{
		acc_sensor_destroy(resources->sensor);
	}
}



static bool acc_initialize_detector_resources(acc_distance_detector_resources_t *resources)
{
	resources->handle = acc_detector_distance_create(resources->config);
	if (resources->handle == NULL)
	{
		printf("acc_detector_distance_create() failed\n");
		return false;
	}

	if (!acc_detector_distance_get_sizes(resources->handle, &(resources->buffer_size), &(resources->detector_cal_result_static_size)))
	{
		printf("acc_detector_distance_get_sizes() failed\n");
		return false;
	}

	resources->buffer = acc_integration_mem_alloc(resources->buffer_size);
	if (resources->buffer == NULL)
	{
		printf("sensor buffer allocation failed\n");
		return false;
	}

	resources->detector_cal_result_static = acc_integration_mem_alloc(resources->detector_cal_result_static_size);
	if (resources->detector_cal_result_static == NULL)
	{
		printf("calibration buffer allocation failed\n");
		return false;
	}

	return true;
}


static bool acc_do_sensor_calibration(acc_sensor_t     *sensor,
                                  acc_cal_result_t *sensor_cal_result,
                                  void             *buffer,
                                  uint32_t         buffer_size)
{
	bool           status              = false;
	bool           cal_complete        = false;
	const uint16_t calibration_retries = 1U;

	// Random disturbances may cause the calibration to fail. At failure, retry at least once.
	for (uint16_t i = 0; !status && (i <= calibration_retries); i++)
	{
		// Reset sensor before calibration by disabling/enabling it
		acc_hal_integration_sensor_disable(1);
		acc_hal_integration_sensor_enable(1);

		do
		{
			status = acc_sensor_calibrate(sensor, &cal_complete, sensor_cal_result, buffer, buffer_size);

			if (status && !cal_complete)
			{
				status = acc_hal_integration_wait_for_sensor_interrupt(1, 2000);
			}
		} while (status && !cal_complete);
	}

	if (status)
	{
		/* Reset sensor after calibration by disabling/enabling it */
		acc_hal_integration_sensor_disable(1);
		acc_hal_integration_sensor_enable(1);
	}

	return status;
}


static bool acc_do_full_detector_calibration(acc_distance_detector_resources_t *resources,
                                         const acc_cal_result_t        *sensor_cal_result)
{
	bool done = false;
	bool status;

	do
	{
		status = acc_detector_distance_calibrate(resources->sensor,
		                                         resources->handle,
		                                         sensor_cal_result,
		                                         resources->buffer,
		                                         resources->buffer_size,
		                                         resources->detector_cal_result_static,
		                                         resources->detector_cal_result_static_size,
		                                         &resources->detector_cal_result_dynamic,
		                                         &done);

		if (status && !done)
		{
			status = acc_hal_integration_wait_for_sensor_interrupt(1, 2000);
		}
	} while (status && !done);

	return status;
}



//typedef struct
//{
//	acc_sensor_t                      *sensor;
//	acc_detector_distance_config_t    *config;
//	acc_detector_distance_handle_t    *handle;
//	void                              *buffer;
//	uint32_t                          buffer_size;
//	uint8_t                           *detector_cal_result_static;
//	uint32_t                          detector_cal_result_static_size;
//	acc_detector_cal_result_dynamic_t detector_cal_result_dynamic;
//} acc_distance_detector_resources_t;


acc_distance_detector_resources_t  save_resources;


static bool acc_do_detector_calibration_update(acc_distance_detector_resources_t *resources,
                                           const acc_cal_result_t        *sensor_cal_result)
{
	bool done = false;
	bool status;

	save_resources=*resources;

	do
	{
		status = acc_detector_distance_update_calibration(resources->sensor,
		                                                  resources->handle,
		                                                  sensor_cal_result,
		                                                  resources->buffer,
		                                                  resources->buffer_size,
		                                                  &resources->detector_cal_result_dynamic,
		                                                  &done);

		if (status && !done)
		{
			status = acc_hal_integration_wait_for_sensor_interrupt(1, 2000);
		}
	} while (status && !done);

	return status;
}


static bool acc_do_detector_get_next(acc_distance_detector_resources_t  *resources,
                                 const acc_cal_result_t         *sensor_cal_result,
                                 acc_detector_distance_result_t *result)
{
	bool result_available = false;

	do
	{
		if (!acc_detector_distance_prepare(resources->handle, resources->config, resources->sensor, sensor_cal_result, resources->buffer,
		                                   resources->buffer_size))
		{
			printf("acc_detector_distance_prepare() failed\n");
			return false;
		}

		if (!acc_sensor_measure(resources->sensor))
		{
			printf("acc_sensor_measure() failed\n");
			return false;
		}

		if (!acc_hal_integration_wait_for_sensor_interrupt(1, 2000))
		{
			printf("Sensor interrupt timeout\n");
			return false;
		}

		if (!acc_sensor_read(resources->sensor, resources->buffer, resources->buffer_size))
		{
			printf("acc_sensor_read() failed\n");
			return false;
		}

		if (!acc_detector_distance_process(resources->handle, resources->buffer, resources->detector_cal_result_static,
		                                   &resources->detector_cal_result_dynamic,
		                                   &result_available, result))
		{
			printf("acc_detector_distance_process() failed\n");
			return false;
		}

		if(A121_distance_config.printf_mode==1)
		{
			acc_print_iq_data(result->sensor_config, result->processing_result, result->processing_metadata);
		}
	} while (!result_available);

	return true;
}



//FIFO
uint16_t  fifo_dist_result[FIFO_NUB];

// 数据从大到小排序
void buf_sort_weight(uint16_t *scores,uint8_t n)
{
	uint8_t i,j;
	uint32_t k;
	for(i=0;i<n;i++)
	{
		for(j=i+1;j<n;j++)
		{
			if(scores[i]<scores[j])
			{
				k=scores[i];
				scores[i]=scores[j];
				scores[j]=k;
			}
		}
	}
}

uint16_t  average_velu(uint16_t *scores,uint8_t n)
{
	uint8_t i;
	uint32_t sun=0;
	for(i=0;i<n;i++)
	{
		sun+=scores[i];
	}
	return sun/n;
}

uint16_t FIFO_data_input(uint16_t dist)
{
	uint8_t i;
	uint32_t add=0;
	uint16_t  disance=0;
	uint16_t  max_min[FIFO_NUB]={0};//从大到小排序
	uint16_t st=0;

	for(i=(FIFO_NUB-1);i>0;i--)
	{
		fifo_dist_result[i]=fifo_dist_result[i-1];
		max_min[i]=fifo_dist_result[i];
	}
	fifo_dist_result[0]=dist;
	max_min[0]=dist;


	if(A121_distance_config.average>7)
	{
		buf_sort_weight(max_min,A121_distance_config.average);
		st=A121_distance_config.average/2-3;
		disance=average_velu(&max_min[st],5);
	}
	else if(A121_distance_config.average>0)
	{
		st=A121_distance_config.average;
		disance=average_velu(&max_min[0],st);
	}
	else
	{
		disance=dist;
	}

	return disance;


}

void print_hex_distance(uint16_t distance)
{
	uint8_t temp[6];
	uint16_t crc_output = 0x00;
	extern CRC_HandleTypeDef hcrc; 
	temp[0]=0xAA;
	temp[1]=0x55;
	temp[2]=distance>>8;
	temp[3]=distance&0xFF;
	// 计算1个uint32_t的CRC，返回值是uint32_t但我们只取低16位
	crc_output = (uint16_t)HAL_CRC_Calculate(&hcrc, (uint32_t*)temp, 4);//CRC-16/CCITT-FALSE
	
	// 低位在前，高位在后
	temp[4]=crc_output&0xFF;
	temp[5]=crc_output>>8;
	HAL_UART_Transmit(&huart1,temp,6,200);
}

uint32_t test_count=0;
static void acc_print_distance_result(const acc_detector_distance_result_t *result)
{
	uint16_t  distance=0;
	uint16_t  T=  result->temperature;

	test_count++;


	//测试增加温度显示
	//	printf("%d detected distances", result->num_distances);
//		if(A121_distance_config.printf_mode==1){return;}
//		if (result->num_distances > 0)
//		{
//			distance=FIFO_data_input((uint16_t)(result->distances[0]*1000));
//			printf("count=%7d,distances[%d]=%5dmm,strs=%.2f",test_count,1,distance,result->strengths[0]);
//			printf(",T=%d",T);
//			if(A121_distance_config.scan_nub>1)
//			{
//				for (uint8_t i = 1; i < A121_distance_config.scan_nub; i++)
//				{
//					printf(",distances[%d]=%5dmm,strs=%.2f",i+1,(uint16_t)(result->distances[i]*1000),result->strengths[i]);
//					printf(",T=%d",T);
//				}
//			}
//			printf("\r\n");
//		}
//		else
//		{
//			printf("count=%7d,distances[%d]=%5dmm,strs=%.2f",test_count,1,0,0);
//			printf(",T=%d",T);
//			for (uint8_t i = 1; i < A121_distance_config.scan_nub; i++)
//			{
//				printf(",distances[%d]=%5dmm,strs=%.2f\n",i+1,0,0);
//				printf(",T=%d",T);
//			}
//			printf("\r\n");
//		}
	//无温度显示///////////////

		if(A121_distance_config.printf_mode==1){return;}
		if (result->num_distances > 0)
		{
			distance=FIFO_data_input((uint16_t)(result->distances[0]*1000));
//			printf("NUB=%d  T=%3d   ",result->num_distances,T);
            if (A121_distance_config.printf_mode==2)
			{
				print_hex_distance((uint16_t)(result->distances[0]*1000));
			}
			else
			{
				printf("distances=%5dmm",(uint16_t)(result->distances[0]*1000));
			}

//			printf("distances[%d]=%5dmm,strs=%3.2f",1,distance,result->strengths[0]);
			// if(A121_distance_config.scan_nub>1)
			// {
			// 	if(A121_distance_config.scan_nub>result->num_distances)
			// 	{
			// 		for (uint8_t i = 1; i < result->num_distances; i++)
			// 		{
			// 			printf(",distances[%d]=%5dmm,strs=%3d.%02d",i+1,(uint16_t)(result->distances[i]*1000),(int16_t)result->strengths[i],(abs)((int16_t)(result->strengths[i]*100))%100);

			// 		}
			// 		for (uint8_t i = result->num_distances; i < A121_distance_config.scan_nub; i++)
			// 		{
			// 			printf(",distances[%d]=%5dmm,strs=%3d.%02d",i+1,0,0,0);
			// 		}
			// 	}
			// 	else
			// 	{
			// 		for (uint8_t i = 1; i < A121_distance_config.scan_nub; i++)
			// 		{
			// 			printf(",distances[%d]=%5dmm,strs=%3d.%02d",i+1,(uint16_t)(result->distances[i]*1000),(int16_t)result->strengths[i],(abs)((int16_t)(result->strengths[i]*100))%100);
			// 		}
			// 	}
			// }
			printf("\r\n");
		}
		else
		{
//			printf("NUB=%d  T=%3d   ",result->num_distances,T);.
            if (A121_distance_config.printf_mode==2)
			{
				print_hex_distance(0);
			}
			else
			{
				printf("distances=%5dmm",0);
			}
			
			// if(A121_distance_config.scan_nub>1)
			// {
			// 	for (uint8_t i = 1; i < A121_distance_config.scan_nub; i++)
			// 	{
			// 		printf(",distances=%5dmm",0);
			// 	}
			// }
			printf("\r\n");
		}

		if(result->near_start_edge_status)
		{
            if (A121_distance_config.printf_mode==2)
			{
				// print_hex_distance(0xFFFF);
			}
			else
			{
				printf("near_start_edge_status\r\n");
			}			
		}
}


static void acc_print_iq_data(const acc_config_t        *sensor_config,
                          acc_processing_result_t   *processing_result,
                          acc_processing_metadata_t *processing_metadata)
{
	/* Get the iq data frame from the processing result */
	acc_int16_complex_t *frame = processing_result->frame;

	/* Get information about the frame from the sensor config */
	uint16_t sweeps_per_frame = acc_config_sweeps_per_frame_get(sensor_config);
	uint8_t  num_subsweeps    = acc_config_num_subsweeps_get(sensor_config);

	if(num_subsweeps<3){return;}
	//printf("BEGIN:Distance(m),Amplitude\n");
	uint16_t iq_nub=0;
	uint16_t length_p[4];
	uint16_t sub_start[4];
	uint16_t sub_end[4];
	uint16_t sub_step[4];

	int32_t  start_point[4];
	uint16_t step_length[4];
	uint16_t subsweep_length[4];
	/* Loop over all subsweeps */
	for (uint8_t subsweep_idx = 0; subsweep_idx < num_subsweeps; subsweep_idx++)
	{
		/* Get information about the start point and step length from the sensor config */
		start_point[subsweep_idx] = acc_config_subsweep_start_point_get(sensor_config, subsweep_idx);
	    step_length[subsweep_idx] = acc_config_subsweep_step_length_get(sensor_config, subsweep_idx);

		/* Get subsweep offset and length from the processing metadata */
		uint16_t subsweep_offset = processing_metadata->subsweep_data_offset[subsweep_idx];
		subsweep_length[subsweep_idx] = processing_metadata->subsweep_data_length[subsweep_idx];

		/* Loop over all points in subsweep */
		for (uint16_t point_idx = 0; point_idx < subsweep_length[subsweep_idx]; point_idx++)
		{
			/* Perform a coherent mean calculation for the point over sweeps per frame */
			uint32_t iq_point_real_acc = 0;
			uint32_t iq_point_imag_acc = 0;

			/* Loop over all points in sweeps_per_frame */
			for (uint16_t sweep_idx = 0; sweep_idx < sweeps_per_frame; sweep_idx++)
			{
				uint16_t point_offset = sweep_idx * subsweep_length[subsweep_idx] + subsweep_offset + point_idx;
				iq_point_real_acc += frame[point_offset].real;
				iq_point_imag_acc += frame[point_offset].imag;
			}

			iq_point_real_acc = iq_point_real_acc / sweeps_per_frame;
			iq_point_imag_acc = iq_point_imag_acc / sweeps_per_frame;

			/* Calculate the absolute value of the IQ point */
			uint32_t iq_point_abs[10]={0};
			iq_point_abs[subsweep_idx] = (uint32_t)sqrt(iq_point_real_acc * iq_point_real_acc + iq_point_imag_acc * iq_point_imag_acc);

			/* Print the point distance, in meters, and the point absolute value */
			//float distance = acc_processing_points_to_meter(start_point + point_idx*step_length);
			//printf("%" PRIfloat ",%" PRIu32 "\n", ACC_LOG_FLOAT_TO_INTEGER(distance), iq_point_abs);
			if(subsweep_idx==0)
			{
				//data_buf[iq_nub++]=iq_point_abs[subsweep_idx];
				//SW_send16_format(iq_point_abs[subsweep_idx],iq_point_abs[1],iq_point_abs[2],iq_point_abs[3]);
				SW_send16_2format(iq_point_abs[subsweep_idx],0);
			}
			else if(subsweep_idx==1)
			{
				length_p[1]=2.5*(start_point[0]+step_length[0]*subsweep_length[0]);
				sub_start[1]=2.5*start_point[1];
				iq_nub=(length_p[1]-sub_start[1])/(step_length[1]*2.5);
				if(point_idx>=iq_nub)
				{
					//data_buf[iq_nub++]=iq_point_abs[subsweep_idx];
					//SW_send16_format(iq_point_abs[subsweep_idx],iq_point_abs[1],iq_point_abs[2],iq_point_abs[3]);
					SW_send16_2format(iq_point_abs[subsweep_idx],0);
				}
			}
			else if(subsweep_idx==2)
			{
				length_p[2]=2.5*(start_point[1]+step_length[1]*subsweep_length[1]);
				sub_start[2]=2.5*start_point[2];
				iq_nub=(length_p[2]-sub_start[2])/(step_length[2]*2.5);
				if(point_idx>=iq_nub)
				{
					//data_buf[iq_nub++]=iq_point_abs[subsweep_idx];
					//SW_send16_format(iq_point_abs[subsweep_idx],iq_point_abs[1],iq_point_abs[2],iq_point_abs[3]);
					SW_send16_2format(iq_point_abs[subsweep_idx],0);
				}
			}
			else if(subsweep_idx==3)
			{
				length_p[3]=2.5*(start_point[2]+step_length[2]*subsweep_length[2]);
				sub_start[3]=2.5*start_point[3];
				iq_nub=(length_p[3]-sub_start[3])/(step_length[3]*2.5);
				if(point_idx>=iq_nub)
				{
					//data_buf[iq_nub++]=iq_point_abs[subsweep_idx];
					SW_send16_2format(iq_point_abs[subsweep_idx],0);
				}
			}
		}
	}
	SW_send16_2format(0,0xfff);
	//printf("END:Distance(m),Amplitude\n");
}
